#include "sphengine.h"
#include "Particle.h"
#include <qmath.h>
#include <QDebug>
#include <limits>

bool validQReal(qreal value){
    if (value != value){
        return false;
    }
    else if (value > std::numeric_limits<qreal>::max()){
        return false;
    }
    else if (value < -std::numeric_limits<qreal>::max()){
        return false;
    }
    else
        return true;
}

SPHEngine::SPHEngine(QList<Particle *> & list, QVector3D *boxm, QVector3D *boxM, qreal _h, qreal _dt) :
    particles(list), dt(_dt), h(_h), k(32), p0(1e3), mu(1.0), sigma(0.0728), m_boxm(boxm), m_boxM(boxM)
{
    memoryInicialization();
}

int       SPHEngine::iX                     (qreal x)
{
    return (x - m_boxm->x())/h;
}

int       SPHEngine::iY                     (qreal y)
{
    return (y - m_boxm->y())/h;
}

int       SPHEngine::iZ                     (qreal z)
{
    return (z - m_boxm->z())/h;
}

void SPHEngine::updateMemory()
{
    for(int i = 0; i < qx; ++i)
    {
        for(int j = 0; j < qy; ++j)
            delete [] grad[i][j];
        delete [] grad[i];
    }
    delete [] grad;

    memoryInicialization();
}

void      SPHEngine::memoryInicialization   ()
{
    qx = qCeil((m_boxM->x() - m_boxm->x())/h);
    qy = qCeil((m_boxM->y() - m_boxm->y())/h);
    qz = qCeil((m_boxM->z() - m_boxm->z())/h);

    grad = new QList<Particle>**[qx];
    for(int i = 0; i < qx; ++i)
    {
        grad[i] = new QList<Particle>*[qy];
        for(int j = 0; j < qy; ++j)
            grad[i][j] = new QList<Particle>[qz];
    }

    QList<int> toRemove;
    for(int i = 0; i < particles.size(); ++i)
    {
        int ix = iX(particles[i]->position().x());
        int iy = iY(particles[i]->position().y());
        int iz = iZ(particles[i]->position().z());

        if( ix >= 0 && ix < qx && iy >= 0 && iy < qy && iz >= 0 && iz < qz)
            grad[ix][iy][iz].append(*particles[i]);
        else
            toRemove.append(i);
    }
    for(int i = toRemove.size()-1; i >=0 ; --i)
        particles.removeAt(toRemove[i]);
}

void SPHEngine::accelerationCalc()
{
	forcesCalc ();
}

void SPHEngine::temporalIntegrationFirstStep()
{
    QList<int> toRemove;
    for(int i = 0; i < particles.size(); ++i)
    {
        int ix = iX(particles[i]->position().x());
        int iy = iY(particles[i]->position().y());
        int iz = iZ(particles[i]->position().z());

        particles[i]->velocity() += particles[i]->acceleration()*dt/2.0;
        particles[i]->position() += particles[i]->velocity    ()*dt;

        colisionDetection(i);

        if(!changePosition(i, ix, iy, iz))
            toRemove.append(i);
    }

    for(int i = toRemove.size()-1; i >=0 ; --i)
        particles.removeAt(toRemove[i]);
}

void SPHEngine::temporalIntegration()
{
    QList<int> toRemove;
	for(int i = 0; i < particles.size(); ++i)
	{
        int ix = iX(particles[i]->position().x());
        int iy = iY(particles[i]->position().y());
        int iz = iZ(particles[i]->position().z());

		particles[i]->velocity() += particles[i]->acceleration()*dt;
		particles[i]->position() += particles[i]->velocity    ()*dt;

        colisionDetection(i);

        if(!changePosition(i, ix, iy, iz))
            toRemove.append(i);
	}

    for(int i = toRemove.size()-1; i >=0 ; --i)
        particles.removeAt(toRemove[i]);
}

bool SPHEngine::changePosition(int i, int ix, int iy, int iz)
{
    int ix2 = iX(particles[i]->position().x());
    int iy2 = iY(particles[i]->position().y());
    int iz2 = iZ(particles[i]->position().z());

    bool r = false;

	for(int j = 0; j < grad[ix][iy][iz].size(); ++j)
        if(grad[ix][iy][iz][j].idx() == particles[i]->idx())
		{
			grad[ix][iy][iz].removeAt(j);
			r = true;
			break;
		}

    if( ix2 >= 0 && ix2 < qx && iy2 >= 0 && iy2 < qy && iz2 >= 0 && iz2 < qz)
    {
        grad[ix2][iy2][iz2].append(*particles[i]);
        return true;
    }
    return false;
}

QVector3D reflect(const QVector3D& I, const QVector3D& N)
{
    return I - 2.0*QVector3D::dotProduct(I,N) * N;
}

void SPHEngine::colisionDetection(int i)
{
    const qreal delta = 0.0001;
    QVector3D offset;
    QVector3D n;
    bool in;
    offset = QVector3D();
    n      = QVector3D();
    in     = false;
    qreal diff;
    // X
    diff = particles[i]->position().x() - m_boxm->x();
    if( diff < 0.0)
    {
        offset.setX( -diff + delta);
        n += QVector3D( 1.0, 0.0 ,0.0);
        in = true;
    }
    diff = particles[i]->position().x()-m_boxM->x();
    if( diff > 0.0)
    {
        offset.setX( -diff - delta);
        n += QVector3D( -1.0, 0.0 ,0.0);
        in = true;
    }

    // Y
    diff = particles[i]->position().y() - m_boxm->y();
    if( diff < 0.0)
    {
        offset.setY( -diff + delta);
        n += QVector3D(0.0,  1.0 ,0.0);
        in = true;
    }
    diff = particles[i]->position().y()-m_boxM->y();
    if( diff > 0.0)
    {
        offset.setY( -diff - delta);
        n += QVector3D( 0.0, -1.0 ,0.0);
        in = true;
    }

    // Z
    diff = particles[i]->position().z() - m_boxm->z();
    if( diff < 0.0)
    {
        offset.setZ( -diff + delta);
        n += QVector3D(0.0, 0.0 , 1.0);
        in = true;
    }
    diff = particles[i]->position().z()-m_boxM->z();
    if( diff > 0.0)
    {
        offset.setZ( -diff - delta);
        n += QVector3D( 0.0, 0.0 , -1.0);
        in = true;
    }

    if(in)
    {
        particles[i]->position() += offset;
        particles[i]->velocity()  = reflect(particles[i]->velocity(), n.normalized());

    }
}

qreal SPHEngine::Wpoli(const QVector3D& r)
{
    return pow((h*h - r.lengthSquared()), 3.0);
}

qreal      SPHEngine::testWpoli              (const QVector3D& r)
{
    return pow((h*h - r.lengthSquared()), 2.0);
}


QVector3D SPHEngine::gradWpoli(const QVector3D& r)
{
    QVector3D ret = pow((h*h - r.lengthSquared()), 2.0)*r;
    return ret;
}
qreal    SPHEngine::laplaWpoli(const QVector3D& r)
{
    qreal f = ( h*h - r.lengthSquared() );
    qreal dx2 = (-4.0*f*r.x()*r.x() + f*f);
    qreal dy2 = (-4.0*f*r.y()*r.y() + f*f);
    qreal dz2 = (-4.0*f*r.z()*r.z() + f*f);
    return dx2+dy2+dz2;
}

qreal      SPHEngine::Wspiky                 (const QVector3D& r)
{
    return pow(h - r.length(), 3);
}

qreal SPHEngine::testWSpiky(const QVector3D& r)
{
    qreal f = (h - r.length());
    return f*f/r.length();
}

QVector3D  SPHEngine::gradWspiky  (const QVector3D& r)
{
    qreal f = (h - r.length());
    return f*f/r.length()*r;
}

qreal  SPHEngine::laplaWvisc  (const QVector3D& r)
{
    return (h - r.length());
}

void SPHEngine::densityAndPressureCalc()
{
    qreal dm = 0.0;
    int cnt = 0;
	for(int i = 0; i < particles.size(); ++i)
	{
        qreal swp = 0.0;
        qreal d = 0.0;
        int ix = iX(particles[i]->position().x());
        int iy = iY(particles[i]->position().y());
        int iz = iZ(particles[i]->position().z());

        int x0 = (ix > 0 )            ? ix-1 : ix;
        int xq = (ix > 0 && ix < qx-1)?  3   : 2 ;

        int y0 = (iy > 0 )            ? iy-1 : iy;
        int yq = (iy > 0 && iy < qy-1)?  3   : 2 ;

        int z0 = (iz > 0 )            ? iz-1 : iz;
        int zq = (iz > 0 && iz < qz-1)?  3   : 2 ;

        for(int jx = x0; jx < x0+xq; ++jx)
        {
            for(int jy = y0; jy < y0+yq; ++jy)
            {
                for(int jz = z0; jz < z0+zq; ++jz)
                {
                    for(int j =0 ; j < grad[jx][jy][jz].size(); ++j)
                    {
                        Particle& pj = grad[jx][jy][jz][j];
                        QVector3D r = particles[i]->position() - pj.position();
                        if(r.length() > h)
                            continue;

                        qreal w = Wpoli(r);
                        swp += w;
                        d += pj.mass()*w;
                        ++cnt;
                    }
                }
            }
        }

        d/=swp;
        dm += d;
        particles[i]->density()  =  d;
        particles[i]->pressure() = k*( pow(particles[i]->density()/p0, 7) -1.0);
	}
}

void SPHEngine::forcesCalc()
{
	for(int i = 0; i < particles.size(); ++i)
	{
		QVector3D fp;
		QVector3D fv;
		QVector3D fs;
		QVector3D n;
        qreal     la = 0.0;
        qreal     lv = 0.0;
        qreal    slv = 0.0;
        qreal    sws = 0.0;
        qreal    slp = 0.0;
        qreal    sgp = 0.0;
        qreal    swp = 0.0;

        int ix = iX(particles[i]->position().x());
        int iy = iY(particles[i]->position().y());
        int iz = iZ(particles[i]->position().z());

		int x0 = (ix > 0 )            ? ix-1 : ix;
		int xq = (ix > 0 && ix < qx-1)?  3   : 2 ;

		int y0 = (iy > 0 )            ? iy-1 : iy;
		int yq = (iy > 0 && iy < qy-1)?  3   : 2 ;

		int z0 = (iz > 0 )            ? iz-1 : iz;
		int zq = (iz > 0 && iz < qz-1)?  3   : 2 ;

		for(int jx = x0; jx < x0+xq; ++jx)
		{
			for(int jy = y0; jy < y0+yq; ++jy)
			{
                for(int jz = z0; jz < z0+zq; ++jz)
				{
					for(int j =0 ; j < grad[jx][jy][jz].size(); ++j)
					{
                        Particle& pj = grad[jx][jy][jz][j];
                        QVector3D r = particles[i]->position() - pj.position();
                        if(r.length() > h)
                            continue;


                        if(pj.idx() != particles[i]->idx())
                        {
                            sws += (testWSpiky(r));
                            QVector3D gs = ( pj.mass()*(particles[i]->pressure()+ pj.pressure())/(2.0*pj.density()))*gradWspiky(r);
                            fp += gs;
                        }


                        lv   = laplaWvisc(r);
                        slv += (lv);
                        fv  += ( pj.mass()*(pj.velocity()-particles[i]->velocity())/pj.density() )       * lv;
                        qreal lp = laplaWpoli(r);
                        slp += (lp);
                        la  += ( pj.mass()/pj.density() )                                                 * lp;
                        qreal gp = testWpoli (r);
                        sgp += (gp);
                        n   += ( pj.mass()/pj.density() )                                                 * gradWpoli(r);
                        qreal wp = Wpoli(r);
                        swp += wp;
					}
				}
			}
		}

        n/=sgp;
        la/=slp;

        qreal lengthN = n.length();
        if(lengthN > 1e-4)
            fs = la * n.normalized();


        fp = -fp/(particles[i]->density()*sws);
        fv = mu*fv/(particles[i]->density()*slv);
        fs = -sigma * fs ;

        if(validQReal(fp.x()) && validQReal(fp.y()) && validQReal(fp.z()))
            particles[i]->addAcceleration( fp );

        if(validQReal(fv.x()) && validQReal(fv.y()) && validQReal(fv.z()))
            particles[i]->addAcceleration( fv );

        if(validQReal(fs.x()) && validQReal(fs.y()) && validQReal(fs.z()))
            particles[i]->addAcceleration( fs );

        particles[i]->addAcceleration(QVector3D(0.0,0.0,-9.80665));


	}
}

void SPHEngine::update()
{
	for(int i = 0; i < particles.size(); ++i)
		particles[i]->resetAcceleration();

	densityAndPressureCalc();
	accelerationCalc();
	temporalIntegration();
}

void SPHEngine::begin()
{
    densityAndPressureCalc();
    accelerationCalc();
    temporalIntegrationFirstStep();
}

